#include "ros/ros.h"
#include "can_msgs/Frame.h"
#include "Cybergear_control.hpp"
#include "Saber_control.hpp"

double offset1=-1.1541;
double offset2=-0.7733;
double offset3=-1.8498;
double offset4=3.0915;
double offset5=0;//代码中会自动更新该参数
double offset6=0;//代码中会自动更新该参数

float motor1_angle=0;
float motor1_angular_velocity=0;
float motor1_moment=0;
float motor2_angle=0;
float motor2_angular_velocity=0;
float motor2_moment=0;
float motor3_angle=0;
float motor3_angular_velocity=0;
float motor3_moment=0;
float motor4_angle=0;
float motor4_angular_velocity=0;
float motor4_moment=0;
float motor5_angle=0;
float motor5_angular_velocity=0;
float motor5_moment=0;
float motor6_angle=0;
float motor6_angular_velocity=0;
float motor6_moment=0;
void do_can_msgs(const can_msgs::Frame::ConstPtr& RxMessage)
{
    uint8_t can_id=0;
    uint8_t uncalibrated_flag=0;
    uint8_t hall_flag=0;
    uint8_t magnetic_flag=0;
    uint8_t overtemperature_flag=0;
    uint8_t overcurrent_flag=0;
    uint8_t undervoltage_flag=0;
    uint8_t mode_state=0;
    float current_angle=0;
    float current_angular_velocity=0;
    float current_moment=0;
    float current_temperature=0;
    ROS_INFO("开始对电机反馈数据进行处理！");
	can_id=(RxMessage->id>>8) & 0xFF;
	uncalibrated_flag=(RxMessage->id>>21) & 0x1;
	hall_flag=(RxMessage->id>>20) & 0x1;
	magnetic_flag=(RxMessage->id>>19) & 0x1;
	overtemperature_flag=(RxMessage->id>>18) & 0x1;
	overcurrent_flag=(RxMessage->id>>17) & 0x1;.
	undervoltage_flag=(RxMessage->id>>16) & 0x1;
	mode_state=(RxMessage->id>>22) & 0x3;
	current_angle=data_conversion(RxMessage->data[0]<<8|RxMessage->data[1],angle_min,angle_max);
	current_angular_velocity=data_conversion(RxMessage->data[2]<<8|RxMessage->data[3],angular_velocity_min,angular_velocity_max);
	current_moment=data_conversion(RxMessage->data[4]<<8|RxMessage->data[5],moment_min,moment_max);
	current_temperature=(RxMessage->data[6]<<8|RxMessage->data[7])/10;
	ROS_INFO("current_moment:%f",current_moment);
    if(abs(current_moment)<12)
    {
	if(can_id==1)
	{
		motor1_angle=current_angle;
		motor1_angular_velocity=current_angular_velocity;
		motor1_moment=current_moment;
        ROS_INFO("motor1_moment:%f",motor1_moment);
        ROS_INFO("motor1_angle:%f",motor1_angle);
	}
	else if(can_id==2)
	{
		motor2_angle=current_angle;
		motor2_angular_velocity=current_angular_velocity;
		motor2_moment=current_moment;
        ROS_INFO("motor2_moment:%f",motor2_moment);
        ROS_INFO("motor2_angle:%f",motor2_angle);
	}
	else if(can_id==3)
	{
		motor3_angle=current_angle;
		motor3_angular_velocity=current_angular_velocity;
		motor3_moment=current_moment;
        ROS_INFO("motor3_moment:%f",motor3_moment);
        ROS_INFO("motor3_angle:%f",motor3_angle);
	}	
	else if(can_id==4)
	{
		motor4_angle=current_angle;
		motor4_angular_velocity=current_angular_velocity;
		motor4_moment=current_moment;
        ROS_INFO("motor4_moment:%f",motor4_moment);
        ROS_INFO("motor4_angle:%f",motor4_angle);
	}
	else if(can_id==5)
	{
		motor5_angle=current_angle;
		motor5_angular_velocity=current_angular_velocity;
		motor5_moment=current_moment;
        ROS_INFO("motor5_moment:%f",motor5_moment);
        ROS_INFO("motor5_angle:%f",motor5_angle);

	}
	else if(can_id==6)
	{
		motor6_angle=current_angle;
		motor6_angular_velocity=current_angular_velocity;
		motor6_moment=current_moment;
        ROS_INFO("motor6_moment:%f",motor6_moment);       
        ROS_INFO("motor6_angle:%f",motor6_angle);
	}
    }
}

extern can_msgs::Frame TxMessage;

int main(int argc, char *argv[])
{
    // can通信之前要执行一下两个命令
    // sudo ip link set can0 type can bitrate 1000000
    // sudo ip link set can0 up
    
    setlocale(LC_ALL,"");

    ros::init(argc,argv,"socketcan_test");

    ros::NodeHandle nh;

    ros::Publisher roscan_send_message = nh.advertise<can_msgs::Frame>("sent_messages",50);

    ros::Subscriber roscan_receive_message = nh.subscribe<can_msgs::Frame>("received_messages",50,do_can_msgs);    

    double motor1_interval=0;
    double motor2_interval=0;
    double motor3_interval=0;
    double motor4_interval=0;
    double motor5_interval=0;
    double motor6_interval=0;

    //延时1秒确保socketcan节点完全启动才可以can通信
    sleep(1);

    ROS_INFO("start send can_messages!");


    //初始化位置
    motor_enable(1,0,roscan_send_message);     sleep(0.1);
    motor_enable(2,0,roscan_send_message);    sleep(0.1);
    motor_enable(3,0,roscan_send_message);    sleep(0.1);
    motor_enable(4,0,roscan_send_message);    sleep(0.1);
    motor_enable(5,0,roscan_send_message);    sleep(0.1);
    motor_enable(6,0,roscan_send_message);    sleep(0.1);
    sleep(1.5);
    ros::spinOnce();
    offset5=motor5_angle;
    offset6=motor6_angle;
    location_mode_write(motor5_id, master1_id,2,offset5,roscan_send_message);
    sleep(0.2);
    ros::spinOnce();
    location_mode_write(motor6_id, master1_id,2,offset6,roscan_send_message);
    sleep(0.2);    
    ros::spinOnce();
    location_mode_write(motor4_id, master1_id,0.1,offset4-PI/2,roscan_send_message);
    sleep(0.5);
    ros::spinOnce();
    location_mode_write(motor1_id, master1_id,0.5,offset1,roscan_send_message);
    sleep(0.5);    
    ros::spinOnce();
    location_mode_write(motor3_id, master1_id,0.5,offset3+2*PI/3,roscan_send_message);
    sleep(1);    
    ros::spinOnce();
    location_mode_write(motor2_id, master1_id,0.5,offset2,roscan_send_message);

    // motor1_interval=(offset1-motor1_angle)/10;
    // motor2_interval=(offset2-motor2_angle)/10;
    // motor3_interval=(offset3+2*PI/3-motor3_angle)/10;
    // motor4_interval=(offset4-PI/2-motor4_angle)/10;
    // for(uint8_t i=1;i<10;i++)
    // {
    //     location_mode_write(motor1_id, master1_id,1,motor1_angle+i*motor1_interval,roscan_send_message);
    //     sleep(1);
    //     ros::spinOnce();
    //     location_mode_write(motor2_id, master1_id,1,motor2_angle+i*motor2_interval,roscan_send_message);
    //     sleep(1);
    //     ros::spinOnce();
    //     location_mode_write(motor3_id, master1_id,1,motor3_angle+i*motor3_interval,roscan_send_message);
    //     sleep(1);
    //     ros::spinOnce();
    //     location_mode_write(motor4_id, master1_id,1,motor4_angle+i*motor4_interval,roscan_send_message);
    //     sleep(1);
    //     ros::spinOnce();
    // }


    while(ros::ok())
    {
        // motor_enable(1,0,roscan_send_message);
        // motor_enable(2,0,roscan_send_message);
        // motor_enable(3,0,roscan_send_message);
        // motor_enable(4,0,roscan_send_message);
        // motor_enable(5,0,roscan_send_message);
        // motor_enable(6,0,roscan_send_message);

        ROS_INFO("message has been sent!");
        ros::Duration(2).sleep();
        ros::spinOnce();
    }

    return 0;
}
